import cv2
import numpy as np


def get_aim_(boxes, classes, color_intrin_part, aligned_depth_frame, shape, object_classify):

    target_xyz_trues, track_cls, Priority, size_cls = [], [], [], []
    center_image = shape
    try:
        for box, cls in zip(boxes, classes):
            if cls == object_classify:
                cx = int((box[0] + box[2]) / 2)
                cy = int((box[1] + box[3]) / 2)
                priority = (center_image[1]//2-cx)**2 + (center_image[0]//2-cy)**2
                target_depth = aligned_depth_frame.get_distance(int(cx), int(cy))
                target_xyz_true = [(cx - color_intrin_part[0]) * target_depth / color_intrin_part[2],
                                   (cy - color_intrin_part[1]) * target_depth / color_intrin_part[3],
                                   target_depth]
                target_xyz_trues.append(target_xyz_true)
                track_cls.append(cls)
                Priority.append(priority)
#            elif:
#                cx = int((box[0] + box[2]) / 2)
#                cy = int((box[1] + box[3]) / 2)
#                priority = (center_image[1] // 2 - cx) ** 2 + (center_image[0] // 2 - cy) ** 2
#                target_depth = aligned_depth_frame.get_distance(int(cx), int(cy))
#                target_xyz_true = [(cx - color_intrin_part[0]) * target_depth / #color_intrin_part[2],
#                                   (cy - color_intrin_part[1]) * target_depth / #color_intrin_part[3],
#                                   target_depth]
#                target_xyz_trues.append(target_xyz_true)
#                track_cls.append(cls)
#                Priority.append(priority)
        # sort
        Priority = np.asarray(Priority, dtype=np.int).reshape(1, -1)
        priority_index = np.arange(0, len(Priority[0])).reshape(1, -1)
        p_index_combine = np.vstack((Priority, priority_index))
        priority_sort_index = np.argsort(p_index_combine[0])
        target_xyz_trues = target_xyz_trues[priority_sort_index[0]]
        track_cls = np.array(track_cls, np.int)
        category = track_cls[priority_sort_index]
    except:
        target_xyz_trues = []
        category = np.array([4], np.int)

    return target_xyz_trues, category


def read_yaml(path):
    infile = cv2.FileStorage(path,cv2.FILE_STORAGE_READ)
    rotate = infile.getNode("Rotate").mat()
    translation = infile.getNode("Translation").mat()
    temp = np.concatenate([rotate,translation],-1)
    eye = [
        [0,0,0,1]
    ]
    eye = np.array(eye)
    out = np.concatenate([temp,eye],0)
    return out


def imagexyz2wordxyz(target_xyz_trues,track_cls,yaml_path):
    data = []
    if target_xyz_trues==[]:
        c, x, y, z = 4, 0, 0, 0
        output = np.array([[0, 0, 0, 0,
                    0, 0, 0, 0,
                    0, 0, 0, 0,
                    0, 0, 0, 0]]).reshape(4,4)
        data.append([c,x,y,z])
        return data, output
    try:
        exter = read_yaml(yaml_path)
    except:
        raise ("eyeToand_calibration.yaml don't exit")
    aim = np.eye(3, 3)
    aim = np.concatenate([aim, np.array(target_xyz_trues).reshape(3,1)], 1)
    aim = np.concatenate([aim, np.array([0, 0, 0, 1]).reshape(1, 4)], 0)
    output = exter.dot(aim)
    x,y,z = output[0,3],output[1,3],output[2,3]
    data.append([int(track_cls[0]), x, y, z])
    return data, output









